#include <iostream>

#include "alg3d/Mat3.h"
#include "alg3d/AxisAngle.h"

namespace gx
{

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat3& Mat3::transpose()
{
    Mat3 copy = *this;
    col_0.y = copy.col_1.x; col_0.z = copy.col_2.x; 
    col_1.x = copy.col_0.y; col_1.z = copy.col_2.y; 
    col_2.x = copy.col_0.z; col_2.y = copy.col_1.z;
    return *this;
}

Mat3& Mat3::invert()
{
    double B[3][3];//the transpose of a matrix A
    double C[3][3];//the adjunct matrix of transpose of a matrix A not adjunct of A
    double X[3][3];//the inverse
    double det = 0.;
  

    for(int i=0,j=0;j<3;j++)
    {     
        if(j==2)
            det+=get_col(i)[j]*get_col(i+1)[0]*get_col(i+2)[1];
        else if(j==1)
            det+=get_col(i)[j]*get_col(i+1)[j+1]*get_col(i+2)[0];
        else
            det+=get_col(i)[j]*get_col(i+1)[j+1]*get_col(i+2)[j+2];
    }
    for(int i=2,j=0;j<3;j++)
    {
        if(j==2)
            det-=get_col(i)[j]*get_col(i-1)[0]*get_col(i-2)[1];
        else if(j==1)
            det-=get_col(i)[j]*get_col(i-1)[j+1]*get_col(i-2)[0];
        else
            det-=get_col(i)[j]*get_col(i-1)[j+1]*get_col(i-2)[j+2];
    }

    double invDet = 1. / det;

    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            B[i][j]=get_col(j)[i];
        }
    }

     C[0][0]=B[1][1]*B[2][2]-(B[2][1]*B[1][2]);
     C[0][1]=(-1)*(B[1][0]*B[2][2]-(B[2][0]*B[1][2]));
     C[0][2]=B[1][0]*B[2][1]-(B[2][0]*B[1][1]);
     
     C[1][0]=(-1)*(B[0][1]*B[2][2]-B[2][1]*B[0][2]);
     C[1][1]=B[0][0]*B[2][2]-B[2][0]*B[0][2];
     C[1][2]=(-1)*(B[0][0]*B[2][1]-B[2][0]*B[0][1]);
 
     C[2][0]=B[0][1]*B[1][2]-B[1][1]*B[0][2];
     C[2][1]=(-1)*(B[0][0]*B[1][2]-B[1][0]*B[0][2]);
     C[2][2]=B[0][0]*B[1][1]-B[1][0]*B[0][1];

    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            X[i][j]=C[i][j]*invDet;
        }
     }

    *this = Mat3(
        X[0][0], X[0][1], X[0][2],
        X[1][0], X[1][1], X[1][2],
        X[2][0], X[2][1], X[2][2]);
    return *this;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

AxisAngle Mat3::to_axis_angle() const
{ 
    assert(0);
    return to_quat().to_axis_angle();
}


Vec3 Mat3::to_euler() const
{
    Real a1,a2,a3;
    Real EPS = 0.001;

    // This is based on the equations found in "Introduction to robotics", by J.Craig, p. 47
    // alpha = a3
    // beta  = a2
    // gamma = a1

    a2 = std::atan2(-col_2[0], 
                    std::sqrt(col_0[0] * col_0[0] 
                            + col_1[0] * col_1[0]));

    if(std::fabs(a2 - PI * .5) < EPS) 
    {
        a1 = std::atan2(col_0[1], col_1[1]);
        a3 = 0.;
    }
    else
    {
        if (fabs(a2 + PI * .5) < EPS) 
        { 
            a1 = -std::atan2(col_0[1], col_1[1]);
            a3 = 0.;
        } 
        else 
        {
            a1 = std::atan2(col_2[1], col_2[2]);
            a3 = std::atan2(col_1[0], col_0[0]);
        }
    }
    // A sign inversion to be compatible with the inverse function...
    return Vec3(-a1, -a2, -a3);
}

Quat Mat3::to_quat() const
{    
    Real const tr = col_0.x + col_1.y + col_2.z;
    Quat quat;

    /* check the diagonal */
    if (tr > 0.0) 
    {
        Real s = sqrt (tr + 1.0);
        quat.w = s / 2.0;
        s = 0.5 / s;
        quat.x = (col_1.z - col_2.y) * s;
        quat.y = (col_2.x - col_0.z) * s;
        quat.z = (col_0.y - col_1.x) * s;
        return quat;
    }
    int i, j, k;
    int nxt[3] = {1, 2, 0};

    /* diagonal is negative */

    i = col_1.y > col_0.x ? 1 : 0;

    i = col_2.z > get_col(i)[i] ?  2 : i;

    j = nxt[i];
    k = nxt[j];

    Real s = sqrt(get_col(i)[i] - (get_col(j)[j] + get_col(k)[k]) + 1.0);
    
    switch(i)
    {   
    case 0:
            quat.x = s * 0.5;
            break;
    case 1:
            quat.y = s * 0.5;
            break;
    case 2:
            quat.z = s * 0.5;
            break;
    }

    if (s > 1e-8)   /* s should be never equal to 0 if matrix is orthogonal */
        s = 0.5 / s;

    quat.w =  (get_col(j)[k] - get_col(k)[j]) * s;

    switch(j)
    {   
    case 0:
        quat.x = (get_col(i)[j] + get_col(j)[i]) * s;
        break;
    case 1:
        quat.y = (get_col(i)[j] + get_col(j)[i]) * s;
        break;
    case 2:
        quat.z = (get_col(i)[j] + get_col(j)[i]) * s;
        break;
    }

    switch(k)
    {   
    case 0:
        quat.x = (get_col(i)[k] + get_col(k)[i]) * s;
        break;
    case 1:
        quat.y = (get_col(i)[k] + get_col(k)[i]) * s;
        break;
    case 2:
        quat.z = (get_col(i)[k] + get_col(k)[i]) * s;
        break;
    }

    
    return quat;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


bool Mat3::is_SR_matrix() const
{
    for(int i = 0; i < 3; ++i)
    {
        for(int j = 0; j < 3; ++j)
        {
            Real const dot = get_col(i).dot(get_col(j)),
                             desired = i == j ? 1. : 0.;
            if(std::fabs(dot - desired) > 1e-8)
            {
                return false;
            }
        }
    }
    return true;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat3 & Mat3::concat(const Mat3 & second)
{
    *this = second * *this;
    return *this;
}

Mat3 operator*(Mat3 const & a, Mat3 const & b)
{
    gx::Vec3 cols[3];
    for(int i=0 ; i<3 ; i++)
    {
        for(int j=0 ; j<3 ; j++)
        {
            cols[j][i] = a.col_0[i] * b.get_col(j)[0] 
                        + a.col_1[i] * b.get_col(j)[1] 
                        + a.col_2[i] * b.get_col(j)[2];
        }
    }
    return Mat3(cols[0], cols[1], cols[2]);
}

Vec3 operator*(const Mat3 & a, const Vec3& v) 
{  
    return Vec3(v.x * a.col_0[0] + v.y * a.col_1[0] + v.z * a.col_2[0],
               v.x * a.col_0[1] + v.y * a.col_1[1] + v.z * a.col_2[1],
               v.x * a.col_0[2] + v.y * a.col_1[2] + v.z * a.col_2[2]);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

bool Mat3::equals(Mat3 const & other, Real tol) const
{	
    return col_0.equals(other.col_0, tol) 
        && col_1.equals(other.col_1, tol)
        && col_2.equals(other.col_2, tol);
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Vec3 Mat3::get_scale() const
{
    return Vec3(col_0.norm(), col_1.norm(), col_2.norm());
}

Mat3 & Mat3::normalize_cols()
{
    col_0.normalize();
    col_1.normalize();
    col_2.normalize();
    return *this;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat3 & Mat3::make_scale(double s)
{
    *this = Mat3(s, 0., 0.,
                0., s, 0.,
                0., 0., s);
    return *this;
}

Mat3 & Mat3::make_scale(const gx::Vec3 & s)
{
    *this = Mat3(s.x, 0., 0., 
                 0., s.y, 0.,
                 0., 0., s.z);
    return *this;
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

Mat3 & Mat3::make_rot(const gx::AxisAngle & aa)
{   
    Real c = std::cos(aa.angle); // in right-handed, ccw rot in left-handed
    Real s = std::sin(aa.angle);
    Real v = 1.0 - c;  // versine(x) = 1 - cos(x)

    const Vec3 & k = aa.axis;

    col_0[0] = k[0] * k[0] * v + c;
    col_0[1] = k[0] * k[1] * v + k[2] * s;
    col_0[2] = k[0] * k[2] * v - k[1] * s;

    col_1[0] = k[1] * k[0] * v - k[2] * s;
    col_1[1] = k[1] * k[1] * v + c;
    col_1[2] = k[1] * k[2] * v + k[0] * s;

    col_2[0] = k[2] * k[0] * v + k[1] * s;
    col_2[1] = k[2] * k[1] * v - k[0] * s;
    col_2[2] = k[2] * k[2] * v + c;

    return *this;
}

Mat3 & Mat3::make_rot(Quat const & q)
{
    Real qnorm2 = 2 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
    Real xx = qnorm2 * q.x,
         yy = qnorm2 * q.y,
         zz = qnorm2 * q.z,
         xy = xx * q.y,
         xz = xx * q.z,
         wx = xx * q.w,
         yz = yy * q.z,
         wy = yy * q.w,
         wz = zz * q.w;
         xx *= q.x;
         yy *= q.y,
         zz *= q.z;

    col_0.x = 1 - yy - zz;
    col_0.y = xy + wz;
    col_0.z = xz - wy;
    
    col_1.x = xy - wz;
    col_1.y = 1 - xx - zz;
    col_1.z = yz + wx;

    col_2.x = xz + wy;
    col_2.y = yz - wx;
    col_2.z = 1 - xx - yy;

    return *this;
}

Mat3 & Mat3::make_rot(Vec3 const & xyz)
{   
    Real aX = xyz[0];
    Real aY = xyz[1];
    Real aZ = xyz[2];
    Real const ci = cos(-aZ),   cj = cos(-aY),   ch = cos(-aX),           
               si = sin(-aZ),   sj = sin(-aY),   sh = sin(-aX),
               cc = ci * ch,      cs = ci * sh,      sc = si * ch,              
               ss = si * sh;
    col_0 = gx::Vec3(cj * ci, sj * cs-sc, sj * cc+ss);
    col_1 = gx::Vec3(cj * si, sj * ss+cc, sj * sc-cs);
    col_2 = gx::Vec3(-sj, cj * sh, cj * ch);
    return *this;
}


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

std::ostream & operator<<(std::ostream &os, const Mat3 &val)
{
    gx::Vec3 col0 = val.get_col_0(),
             col1 = val.get_col_1(),
             col2 = val.get_col_2();
    os << zero_if_tiny(col0.x) << " " << zero_if_tiny(col1.x) << " " 
       << zero_if_tiny(col2.x) << " " << "\n";
    os << zero_if_tiny(col0.y) << " " << zero_if_tiny(col1.y) << " " 
       << zero_if_tiny(col2.y) << " " << "\n";
    os << zero_if_tiny(col0.z) << " " << zero_if_tiny(col1.z) << " " 
       << zero_if_tiny(col2.z) << " " << "\n";
    return os;
}

std::istream & operator>>(std::istream &is, Mat3 &val)
{
    Real r00, r01, r02, r10, r11, r12, r20, r21, r22;
    is >>	r00 >> r01 >> r02 >> 
            r10 >> r11 >> r12 >> 
            r20 >> r21 >> r22;
    val = gx::Mat3(	r00, r01, r02,
                    r10, r11, r12,
                    r20, r21, r22);
    return is;
}

} // namespace gx


